#ifndef _SPLINE_CPP_
#define _SPLINE_CPP_

#include "Spline.h"

template<int Dim,int Order,SplineType t>
Spline<Dim,Order,t>::Spline(int N,vector<double> t1)
{
  if(Dim==1) s.resize(N);
  if(Dim==2) s1.resize(N);
  T=t1;
}

template<int Dim,int Order,SplineType t>
Spline<Dim,Order,t>::Spline(int N)
{
  if(Dim==1) s.resize(N);
  if(Dim==2) s1.resize(N);
}

template<int Dim,int Order,SplineType t>
Polynomial<4,double> Spline<Dim,Order,t>::B3(int i,int j)
{
  switch(j){
  case 1:{
    vector<double> arr={1-i,1};
    Polynomial<2,double> p(arr);
    Polynomial<4,double> P;
    P=(1.0/6)*p*p*p;
    return P;
  }break;
  case 2:{
    vector<double> arr={1-i,1};
    vector<double> arr1={1+i,-1};
    Polynomial<2,double> p1(arr);
    Polynomial<2,double> p2(arr1);
    Polynomial<4,double> P;
    P=(2.0/3)+(-1.0/2)*p1*p2*p2;
    return P;
  }break;
  case 3:{
    vector<double> arr={3+i,-1};
    vector<double> arr1={-1-i,1};
    Polynomial<2,double> p1(arr);
    Polynomial<2,double> p2(arr1);
    Polynomial<4,double> P;
    P=(2.0/3)+(-1.0/2)*p1*p2*p2;
    return P;
  }break;
  case 4:{
    vector<double> arr={3+i,-1};
    Polynomial<2,double> p(arr);
    Polynomial<4,double> P;
    P=(1.0/6)*p*p*p;
    return P;
  }break;
  default:cout<<"ERROR!"<<endl;
  }
  vector<double> arr={0,0,0,0};
  Polynomial<4,double> p(arr);
  return p;
}

template<int Dim,int Order,SplineType t>
Polynomial<3,double> Spline<Dim,Order,t>::B2(int i,int j)
{
  switch(j){
  case 1:{
    vector<double> arr={1-i,1};
    Polynomial<2,double> p(arr);
    Polynomial<3,double> P;
    P=(1.0/2)*p*p;
    return P;
  }break;
  case 2:{
    vector<double> arr={-i-1.0/2,1};
    Polynomial<2,double> p(arr);
    Polynomial<3,double> P;
    P=3.0/4+(-1.0)*p*p;
    return P;
  }break;
  case 3:{
    vector<double> arr={2+i,-1};
    Polynomial<2,double> p(arr);
    Polynomial<3,double> P;
    P=(1.0/2)*p*p;
    return P;
  }break;
  default:cout<<"ERROR!"<<endl;
  }
  vector<double> arr={0,0,0};
  Polynomial<3,double> p(arr);
  return p;
}



template<int Ord>
Spline<1,Ord,ppForm> interpolate(const InterConditions &p,BCType Type)
{
  vector<double> X=p.get_x();
  vector<double> f=p.get_f();
  int n=X.size();
  double h[n-1];
  for(int i=0;i<n-1;i++)
    h[i]=X[i+1]-X[i];
  
  if(Ord==4){
  double A[n][n]={0};
  for(int i=0;i<n;i++)
    for(int j=0;j<n;j++)
      A[i][j]=0;
  for(int i=1;i<n-1;i++)
    {
      A[i][i-1]=h[i-1];
      A[i][i]=2*(h[i-1]+h[i]);
      A[i][i+1]=h[i];
    }
  double b[n]={0};
  for(int i=1;i<n-1;i++)
    b[i]=6*((f[i+1]-f[i])/h[i]-(f[i]-f[i-1])/h[i-1]);
  int ipiv[n];
   
  switch(Type)
    {
    case complete:{
      A[0][0]=2*h[0];A[0][1]=h[0];
      A[n-1][n-2]=h[n-2];A[n-1][n-1]=2*h[n-2];
      b[0]=6*((f[1]-f[0])/h[0]-f[n]);
      b[n-1]=6*(f[n+1]-(f[n-1]-f[n-2])/h[n-2]);
      LAPACKE_dgesv(LAPACK_ROW_MAJOR,n,1,A[0],n,&ipiv[0],&b[0],1);
    }break;
    case notAknot:{
      A[0][0]=-h[1];A[0][1]=h[0]+h[1];A[0][2]=-h[0];
      A[n-1][n-3]=-h[n-2];A[n-1][n-2]=h[n-2]+h[n-3];A[n-1][n-1]=-h[n-3];
      b[0]=0;
      b[n-1]=0;
      LAPACKE_dgesv(LAPACK_ROW_MAJOR,n,1,A[0],n,&ipiv[0],&b[0],1);
    } break;
    case periodic:{
      A[0][0]=2*h[0];A[0][1]=h[0];
      A[0][n-2]=h[n-2];A[0][n-1]=2*h[n-2];
      A[n-1][0]=1;A[n-1][n-1]=-1;
      b[0]=6*((f[1]-f[0])/h[0]-(f[n-1]-f[n-2])/h[n-2]);
      b[n-1]=0;
      LAPACKE_dgesv(LAPACK_ROW_MAJOR,n,1,A[0],n,&ipiv[0],&b[0],1);
    }break;
    default:cout<<"ERROR!"<<endl;break;
    }
  Spline<1,Ord,ppForm> S(n,X);
  for(int i=1;i<n;i++)
    {
      double a=f[i-1];
      double b1=(f[i]-f[i-1])/h[i-1]-h[i-1]*b[i-1]/2-h[i-1]*(b[i]-b[i-1])/6;
      double c=b[i-1]/2;
      double d=(b[i]-b[i-1])/(6*h[i-1]);
      vector<double> arr(2,0);
      if(X[i-1]!=0)
	arr[0]=-X[i-1];
      else
	arr[0]=X[i-1];
      arr[1]=1;
      Polynomial<2,double> s0(arr);
      S.s[i]=a+b1*s0+c*s0*s0+d*s0*s0*s0;	
    }
  return S;}
  if(Ord==2)
    {
      double H[n-1];
      for(int i=0;i<n-1;i++)
	H[i]=f[i+1]-f[i];
      Spline<1,Ord,ppForm> S(n,X);
      vector<double> arr(Ord,0);
      for(int i=1;i<n;i++)
	{
	  arr[0]=f[i-1]-X[i-1]*H[i-1]/h[i-1];
	  arr[1]=H[i-1]/h[i-1];
	  Polynomial<Ord,double> s0(arr);
	  S.s[i]=s0;
	}
      return S;
    }
}


template<int Ord>
Spline<1,Ord,cardinalB> interpolate(const InterConditions &p,BCTYPE Type)
{
  vector<double> X=p.get_x();
  vector<double> f=p.get_f();
  int n=X.size();
  double A[n][n]={0};
  double b[n]={0};
  for(int i=0;i<n;i++)
    for(int j=0;j<n;j++)
      A[i][j]=0;
  int ipiv[n];
  double x[n+2];
  switch(Type){
  case cubic:{
    for(int i=0;i<n;i++)
      {
	A[i][i]=4;
	A[i+1][i]=1;
	A[i][i+1]=1;
      }
    A[0][1]=2;A[n-1][n-2]=2;
    for(int i=1;i<n-1;i++)
      b[i]=6*f[i];
    b[0]=6*f[0]+2*f[n];b[n-1]=6*f[n-1]-2*f[n+1];
    LAPACKE_dgesv(LAPACK_ROW_MAJOR,n,1,A[0],n,&ipiv[0],&b[0],1);
    x[0]=b[1]-2*f[n];x[n+1]=b[n-2]+2*f[n+1];
    for(int i=0;i<n;i++)
      x[i+1]=b[i];
    Spline<1,Ord,cardinalB> S(n,X);
    double c;int d;
    for(int i=1;i<n;i++)
      {
	c=X[i-1]-2;
	d = static_cast<int>(c);
	S.s[i]=(x[i-1]*S.B3(d,4))+(x[i]*S.B3(d+1,3))+(x[i+1]*S.B3(d+2,2))+(x[i+2]*S.B3(d+3,1));
      }
    return S;}break;
  case quadratic:
    {
      for(int i=0;i<n;i++)
	{
	  A[i][i]=6;
	  A[i+1][i]=1;
	  A[i][i+1]=1;
	}
      A[0][0]=5;A[n-1][n-1]=5;
      for(int i=1;i<n-1;i++)
	b[i]=8*f[i];
      b[0]=8*f[0]-2*f[n];b[n-1]=8*f[n-1]-2*f[n+1];
      LAPACKE_dgesv(LAPACK_ROW_MAJOR,n,1,A[0],n,&ipiv[0],&b[0],1);
      x[0]=2*f[n]-b[0];x[n+1]=2*f[n+1]-b[n-1];
      for(int i=0;i<n;i++)
	x[i+1]=b[i];
      Spline<1,Ord,cardinalB> S(n+1,X);
      double c;int d;
      for(int i=1;i<=n;i++)
	{
	  if(X[i-1]>1)
	    c=X[i-1]-1;
	  else
	    c=X[i-1]-2;
	  d = static_cast<int>(c);
	  S.s[i]=(x[i-1]*S.B2(d,3))+(x[i]*S.B2(d+1,2))+(x[i+1]*S.B2(d+2,1));
	}
      return S;
    }break;
  }
}


template<int Ord> 
Spline<2,Ord,ppForm> fitCurve(const vector<Vec<double,2> > &p,BCType Type)
{
  int n=p.size();
  vector<double> t(n,0);
  int N=n;
  switch(Type){
  case complete:{
    N=n-2;
    t.resize(n-2);
    for(int i=1;i<n-2;i++)
      {
	Vec<double,2> p11=p[i];
	Vec<double,2> p12=p[i-1];
	t[i]=t[i-1]+norm(p11-p12,2);
      }
  }break;
  case notAknot:case periodic:{
  for(int i=1;i<n;i++)
    {
      Vec<double,2> p11=p[i];
      Vec<double,2> p12=p[i-1];
      t[i]=t[i-1]+norm(p11-p12,2);
    }
  }break;
  }
  vector<double> x(n);
  vector<double> y(n);
  for(int i=0;i<n;i++)
    {
      x[i]=p[i][0];
      y[i]=p[i][1];
    }
  InterConditions p1(N,t,x);
  InterConditions p2(N,t,y);
  Spline<1,Ord,ppForm> S1=interpolate<Ord>(p1,Type);
  Spline<1,Ord,ppForm> S2=interpolate<Ord>(p2,Type);
  Spline<2,Ord,ppForm> S(N,t);
  for(int i=1;i<N;i++)
    {
      vector<double> Coef1=S1.s[i].get_Coef();
      vector<double> Coef2=S2.s[i].get_Coef();
      vector<Vec<double,2> > Coef0(Ord);
      for(int j=0;j<Ord;j++)
	{
	  Coef0[j][0]=Coef1[j];
	  Coef0[j][1]=Coef2[j];
	}
      Polynomial<Ord,Vec<double,2> > p0(Coef0);
      S.s1[i]=p0;
    }
  return S;
}




template<int Dim,int Order,SplineType t>
double Spline<Dim,Order,t>::s_get_value(double x)
{
  int index;
  int n=T.size();
  switch(Order){
  case 4:{for(int i=0;i<n;i++)
    if(x >= T[i] && x<=T[i+1])
      {
	index=i+1;
	break;
      }
      return s[index].get_value(x);}break;
  case 3:{
    vector<double> TT(n+1);
    for(int i=0;i<n;i++)
      if(T[i]<0)
	TT[i]=static_cast<int>(T[i]-1);
      else
	TT[i]=static_cast<int>(T[i]);
    TT[n]=TT[n-1]+1;
    for(int i=0;i<n;i++)
     if(x >= TT[i] && x<=TT[i+1])
      {
	index=i+1;
	break;
      }
    return s[index].get_value(x);}break;
  default:cout<<"ERROR!"<<endl;
  }
}

template<int Dim,int Order,SplineType t>
Vec<double,2> Spline<Dim,Order,t>::s1_get_value(double x)
{
  int n=T.size();
  int index;
  for(int i=0;i<n;i++)
    if(x >= T[i] && x<=T[i+1])
      {
	index=i+1;
	break;
      }
  return s1[index].get_value(x);
}











#endif
